

    .option norvc
    .section .text.init,"ax",@progbits
  
    .globl bl2_entrypoint
bl2_entrypoint:
    
    // atf_state_set x28, x29, ATF_STATE_BL2_ENTRY_POINT
    // debug trace ???? 
    addiw   t3, zero, 89
    slli    t3, t3, 0x19        # 0xB2000000
    lui     t4, 0x3000
    sw      t3, 132(t4)         # 3000084

    
    ### write mtvec and make sure it sticks
    la      t0, trap_vector
    csrw    mtvec, t0

    ### set mxstatus to init value
    li      t1, 0xc0638000
    csrw    mxstatus, t1

    ### set plic_ctrl = 1,  plic_base = mapbaddr = 0x7000-0000
    li      t0, 0x701FFFFC        # plic_base + 0x1FFFFC
    li      x4, 1
    sw      x4, 0(t0)

    ### invalid I-cache
    li      t0, 0x33
    csrc    mcor, t0
    li      t0, 0x11
    csrs    mcor, t0

    ### enable I-cache
    li      t0, 0x1
    csrs    mhcr, t0

    ### invalid D-cache
    li      t0, 0x33
    csrc    mcor, t0
    li      t0, 0x12
    csrs    mcor, t0

    ### enable D-cache
    li      t0, 0x2
    csrs    mhcr, t0

    ### zero bss section
    la      a3, __BSS_START__
    la      a4, __BSS_END__
    sub     a4, a4, a3
    beqz    a4, all_ready

bss_clear:
    sd      zero, 0(a3)
    addi    a3, a3, 8
    addi    a4, a4, -8
    bnez    a4, bss_clear

all_ready:
    
    ### stack top = 0x84000000 
    lui     sp, 0x84000
    csrw    mscratch, sp
    lui     sp, 0x83f00

    // 
    call    SystemInit
    call    main
    call    sys_reset

die_loop:
    nop
    j       die_loop

    

    .balign 4
trap_vector:
    
    // switch stack pointer
    csrrw    sp, mscratch, sp
    addi    sp, sp, -128

    // save regs...
    sd      ra, 0(sp)
    
    sd      a0, 8(sp)
    sd      a1, 16(sp)
    sd      a2, 24(sp)
    sd      a3, 32(sp)
    sd      a4, 40(sp)
    sd      a5, 48(sp)
    sd      a6, 56(sp)
    sd      a7, 64(sp)
    
    sd      t0, 72(sp)
    sd      t1, 80(sp)
    sd      t2, 88(sp)
    sd      t3, 96(sp)
    sd      t4, 104(sp)
    sd      t5, 112(sp)
    sd      t6, 120(sp)

    // 判断最高 bit 是否为 1, 
    csrr    a0, mcause
    blt     a0, zero, intproc
    mv      a1, sp
    call    trap_handler
    jal     zero, ctx_pops

intproc:
    andi    a0, a0, 0x1f
    call    intr_handler

ctx_pops:
    // restore regs...
    ld      ra, 0,(sp)
    
    ld      a0, 8(sp)
    ld      a1, 16(sp)
    ld      a2, 24(sp)
    ld      a3, 32(sp)
    ld      a4, 40(sp)
    ld      a5, 48(sp)
    ld      a6, 56(sp)
    ld      a7, 64(sp)
    
    ld      t0, 72(sp)
    ld      t1, 80(sp)
    ld      t2, 88(sp)
    ld      t3, 96(sp)
    ld      t4, 104(sp)
    ld      t5, 112(sp)
    ld      t6, 120(sp)

    //
    addi    sp, sp, 128
    csrrw    sp, mscratch, sp
    mret


    
    

